/*******************************************************************************
* File Name          : MinIMU_V2.c
* Author             :
* Author             :
* Version            : 1.0
* Date               : 5/15/2013
* Description        : MinIMU_V2 driver file
*
* HISTORY:
* Date               |	Modification                    |	Author

*******************************************************************************/


/* Includes ------------------------------------------------------------------*/
#include "minimu_v2.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define Samples          0x0C
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
float32_t *DCM[9];
AxesRaw_t Gyro;
AxesRaw_t Accel;
AxesRaw_t Mag;
float32_t *OmegaPropCorr[3];
float32_t *OmegaIntCorr[3];
float32_t *KpRollPitch;
float32_t *Heading;
u8_t *Data;

/* Private function prototypes -----------------------------------------------*/
void dot_prod_f32(float32_t * A, float32_t * B, float32_t * result);
void cross_prod_f32(float32_t * A, float32_t * B, float32_t * result);
void scale_f32(float32_t * A, float32_t scale, float32_t * result);
void add_f32(float32_t * A, float32_t * B, float32_t * result);
void add_f32A(AxesRaw_t *A, float32_t *B, float32_t *result);
void sub_f32(float32_t * A, float32_t * B, float32_t * result);
float32_t DegtoRad(float32_t x);
float32_t RadtoDeg(float32_t x);

/* Private functions --------------------------------------------------------*/
/*******************************************************************************
* Function Name     : Initialization
* Description       : Initialize both L3GD20 and LSM303DLHC integrated circuits
* Input		    : None
* Output	    : None
* Return	    : None
*******************************************************************************/
void MinIMU_V2_Init() {

	//L3GD20 Initialization
	L3GD20_Init();
	//LSM303DLHC Initialization
	LSM303DLHC_Init();
	//BMP085 Initialization
	BMP085_Init();
}

/*******************************************************************************
* Function Name		: L3GD20_ReadBuffer
* Description		: Generic Reading function. It must be full-filled with either
*					: I2C or SPI reading functions
* Input				: Starting Register Address
* Output			: Data Buffer Read
* Return			: None
*******************************************************************************/
void AHRS() {

	L3GD20_GetAngRateDeg(&Gyro);
	LSM303DLHC_GetAccAxes(&Accel);
	LSM303DLHC_GetMagAxes(&Mag);
}

/*******************************************************************************
* Function Name		: L3GD20 Read Who Am I Register
* Description		: Generic Reading function. It must be full-filled with either
*					: I2C or SPI reading functions
* Input				: Who Am I Register Address
* Output			: Who Am I Register
* Return			: None
*******************************************************************************/
void WhoAmI(u8_t* Data) {

	L3GD20_ReadReg(L3GD20_WHO_AM_I , Data);
}

/*******************************************************************************
* Function Name		: updateDCM
* Description		: Update DCM Rotation Matrix
* Input				: 
* Output			: 
* Return			: None
*******************************************************************************/
void updateDCM() {
	
	assert( FLT_EVAL_METHOD == 2 );

	float32_t OmegaGyro[3] = { 0, 0, 0 };
	float32_t Velocity[3] = { 0, 0, 0 };     //1st position to be GPS_Groundspeed
	float32_t gReference[3] = { 0, 0, 0 };
	float32_t rollpitchCorrPlane[3] = { 0, 0, 0 };
	float32_t totCorr[3] = { 0, 0, 0 };
	float32_t GPS_Course = 0;
	
	L3GD20_GetAngRateRad(&Gyro);
	
	//Update DCM
	float32_t XRow[3] = { 1, -Gyro.AXIS_Z, Gyro.AXIS_Y };
	float32_t YRow[3] = { Gyro.AXIS_Z, 1, -Gyro.AXIS_X };
	float32_t ZRow[3] = { -Gyro.AXIS_Y, Gyro.AXIS_X, 1 };
	dot_prod_f32(DCM[0], XRow, DCM[0]);
	dot_prod_f32(DCM[3], YRow, DCM[1]);
	dot_prod_f32(DCM[6], ZRow, DCM[2]);
	dot_prod_f32(DCM[0], XRow, DCM[3]);
	dot_prod_f32(DCM[3], YRow, DCM[4]);
	dot_prod_f32(DCM[6], ZRow, DCM[5]);
	dot_prod_f32(DCM[0], XRow, DCM[6]);
	dot_prod_f32(DCM[3], YRow, DCM[7]);
	dot_prod_f32(DCM[6], ZRow, DCM[8]);
	
	//Normalize
	float error = 0.0;
	float XOrtho[3] = { 0, 0, 0 };
	float YOrtho[3] = { 0, 0, 0 };
	float ZOrtho[3] = { 0, 0, 0 };
	float tmpScalar[3] = { 0, 0, 0 };
	dot_prod_f32(DCM[0], DCM[3], &error);                                              //eq 18
	//XOrtho = X - (err/2)Y
	scale_f32(DCM[3], -error/2, &tmpScalar[0]);                                        //eq 19
	sub_f32(DCM[0], &tmpScalar[0], &XOrtho[0]);
	//YOrtho = Y - (err/2)X
	scale_f32(DCM[0], -error/2, &tmpScalar[0]);                                        //eq 19
	sub_f32(DCM[3], &tmpScalar[0], &YOrtho[0]);
	//ZOrtho = XOrtho x YOrtho
	cross_prod_f32(&XOrtho[0], &YOrtho[0], &ZOrtho[0]);                                 //eq 20
	
	//Drift Cancellation
	float yawCorrEarth = 0;
	float yawCorrPlane[3] = { 0, 0, 0 };
	
	float COGX = sinf(DegtoRad(GPS_Course));                                            //eq 22
	float COGY = cosf(DegtoRad(GPS_Course));                                            //eq 22

	yawCorrEarth = *DCM[0] * COGY - *DCM[3] * COGX;                                     //eq 23
	scale_f32(DCM[6], yawCorrEarth, &yawCorrPlane[0]);                                  //eq 24
	
	//Roll/Pitch Correction
	float ACentrifugal[3] = { 0, 0, 0 };
	cross_prod_f32(&OmegaGyro[0], &Velocity[0], &ACentrifugal[0]);                      //eq 25
	add_f32A(&Accel, &ACentrifugal[0], &gReference[0]);                                //eq 26
	cross_prod_f32(DCM[6], &gReference[0], &rollpitchCorrPlane[0]);                     //eq 27
	
	//Feedback Controller
	add_f32(&rollpitchCorrPlane[0], &yawCorrPlane[0], &totCorr[0]);                     //eq 28
	scale_f32(&totCorr[0], *KpRollPitch, OmegaPropCorr[0]);
	add_f32(OmegaIntCorr[0], OmegaPropCorr[0], OmegaIntCorr[0]);                        //eq 29
	
	//Euler Angles
	float Pitch = RadtoDeg(-asinf(*DCM[6]));
	float Roll = RadtoDeg(atan2f(*DCM[7], *DCM[8]));
	//float Yaw = RadtoDeg(atan2f(*DCM[3], *DCM[0]));
	
	//Calculate Heading
	float HeadX, HeadY, Heading, sinPitch = sinf(Pitch), cosPitch = cosf(Pitch);
	float sinRoll = sinf(Roll), cosRoll = cosf(Roll);

	//Tilt-compensated Magnetic field X component
	HeadX = (Mag.AXIS_X * (cosPitch)) + (Mag.AXIS_Y * (sinRoll) * (sinPitch)) + (Mag.AXIS_Z * (cosRoll) * (sinPitch));
	//Tilt-compensated Magnetic field Y component
	HeadY = (Mag.AXIS_Y * cosRoll) - (Mag.AXIS_Z * sinRoll);
	//Magnetic Heading
	Heading = atan2(-HeadY, HeadX);
	
	//Magnetic Declination = 7.6 min W per year
	// http://www.ngdc.noaa.gov/geomag-web/#declination
	if (MAG_DECLINATION != 0)
	{
		Heading = Heading + DegtoRad(MAG_DECLINATION);
		if (Heading > PI)
			Heading -= (2.0f * PI);
		else if (Heading < -PI)
			Heading += (2.0f * PI);
	}
	
	Heading = RadtoDeg(Heading);
}

/*******************************************************************************
* Function Name : Build Orientation String
* Description : Dot product of floating-point vectors.
* Input : *pSrcA points to the first input vector
* Input : *pSrcB points to the second input vector
* Input : blockSize number of samples in each vector
* Output : *result output result returned here
* Return : None
*******************************************************************************/
status_t getOrientationString(char *str)
{
	u8_t cnt = 0;

	strcpy(str, "Gyro: ");
	cnt += 6;
	cnt += snprintf(str + cnt, 7, "%.2f", Gyro.AXIS_X);
	strcat(str, ",");
	cnt++;
	cnt += snprintf(str + cnt, 7, "%.2f", Gyro.AXIS_Y);
	strcat(str, ",");
	cnt++;
	cnt += snprintf(str + cnt, 7, "%.2f", Gyro.AXIS_Z);
	strcat(str, ",");
	cnt++;
	strcat(str, "Accel: ");
	cnt += 7;
	cnt += snprintf(str + cnt, 7, "%.2f", Accel.AXIS_X);
	strcat(str, ",");
	cnt++;
	cnt += snprintf(str + cnt, 7, "%.2f", Accel.AXIS_Y);
	strcat(str, ",");
	cnt++;
	cnt += snprintf(str + cnt, 7, "%.2f", Accel.AXIS_Z);
	strcat(str, ",");
	cnt++;
	strcat(str, "Mag: ");
	cnt += 5;
	cnt += snprintf(str + cnt, 7, "%.2f", Mag.AXIS_X);
	strcat(str, ",");
	cnt++;
	cnt += snprintf(str + cnt, 7, "%.2f", Mag.AXIS_Y);
	strcat(str, ",");
	cnt++;
	cnt += snprintf(str + cnt, 7, "%.2f", Mag.AXIS_Z);

	return MEMS_SUCCESS;
}

/*******************************************************************************
* Function Name : Dot Product
* Description : Dot product of floating-point vectors.
* Input : *pSrcA points to the first input vector
* Input : *pSrcB points to the second input vector
* Input : blockSize number of samples in each vector
* Output : *result output result returned here
* Return : None
*******************************************************************************/
void dot_prod_f32(float32_t *A, float32_t *B, float32_t *result)
{
 *result = A[0] * B[0];
 *result += A[1] * B[1];
 *result += A[2] * B[2];
}

/*******************************************************************************
* Function Name : Cross Product
* Description : Cross product of floating-point vectors.
* Input : *pSrcA points to the first input vector
* Input : *pSrcB points to the second input vector
* Input : blockSize number of samples in each vector
* Output : *result output result returned here
* Return : None
*******************************************************************************/
void cross_prod_f32(float32_t *A, float32_t *B, float32_t *result)
{
result[0] = A[1] * B[2] - A[2] * B[1];
result[1] = A[2] * B[0] - A[0] * B[2];
result[2] = A[0] * B[1] - A[1] * B[0];
}

/*******************************************************************************
* Function Name : Scalar Product
* Description : Scalar product of floating-point vectors.
* Input : *pSrcA points to the first input vector
* Input : *pSrcB points to the second input vector
* Input : blockSize number of samples in each vector
* Output : *result output result returned here
* Return : None
*******************************************************************************/
void scale_f32(float32_t *A, float32_t scale, float32_t *result)
{
result[0] = A[0] * scale;
result[1] = A[1] * scale;
result[2] = A[2] * scale;
}

/*******************************************************************************
* Function Name : Add Vectors
* Description : Addition of floating-point vectors.
* Input : *pSrcA points to the first input vector
* Input : *pSrcB points to the second input vector
* Input : blockSize number of samples in each vector
* Output : *result output result returned here
* Return : None
*******************************************************************************/
void add_f32(float32_t *A, float32_t *B, float32_t *result)
{
result[0] = A[0] + B[0];
result[1] = A[1] + B[1];
result[2] = A[2] + B[2];
}

void add_f32A(AxesRaw_t *A, float32_t *B, float32_t *result)
{
result[0] = A->AXIS_X + B[0];
result[1] = A->AXIS_Y + B[1];
result[2] = A->AXIS_Z + B[2];
}

/*******************************************************************************
* Function Name : Subtract Vectors
* Description : Subtraction of floating-point vectors.
* Input : *pSrcA points to the first input vector
* Input : *pSrcB points to the second input vector
* Input : blockSize number of samples in each vector
* Output : *result output result returned here
* Return : None
*******************************************************************************/
void sub_f32(float32_t *A, float32_t *B, float32_t *result)
{
result[0] = A[0] - B[0];
result[1] = A[1] - B[1];
result[2] = A[2] - B[2];
}

/*******************************************************************************
* Function Name : DegtoRad
* Description : Convert a floating point number from degrees to radians.
* Input : (x) input value in degrees.
* Return : (x) value in radians.
*******************************************************************************/
float32_t DegtoRad(float32_t x)
{
return (x*(PI/180));
}

/*******************************************************************************
* Function Name : RadtoDeg
* Description : Convert a floating point number from radians to degrees.
* Input : (x) input value in radians.
* Return : (x) value in degrees.
*******************************************************************************/
float32_t RadtoDeg(float32_t x)
{
return (x*(180/PI));
}
